#include "vmc.h"

/* 运动学正解 */
void state_solution(VMC_t *in, float sita1, float sita2, float *x, float *z)
{
    float l = L1 * cosf((sita2 - sita1) / 2) + sqrtf(L2 * L2 - L1 * L1 * sinf((sita2 - sita1) / 2) * sinf((sita2 - sita1) / 2));
    float gama = (sita2 + sita1) / 2;

    *x = l * cosf(gama);
    *z = -l * sinf(gama);
}

/* 运动学逆解 */
void state_inv_solution(VMC_t *in, float x, float z, float *sita1, float *sita2)
{
    /* 足端在中心交点的虚拟腿转角 */
    // float sita = _PI_2 - atan2f(-z, x);
    // if (sita > MAX_A)
    // {
    //     sita = MAX_A;
    //     UART_Print("less than the minimum angle\n");
    // }
    // else if (sita < MIN_A)
    // {
    //     sita = MIN_A;
    //     UART_Print("larger than the maximum angle\n");
    // }

    // /* 虚拟腿长 */
    // float r = sqrtf(z * z + x * x);
    // if (r > MAX_L)
    // {
    //     r = MAX_L;
    //     UART_Print("Longer than the longest leg\n");
    // }
    // else if (r < MIN_L)
    // {
    //     r = MIN_L;
    //     UART_Print("Shorter than the shortest leg\n");
    // }

    // /* 前后半腿夹角的一半 */
    // float phi = acosf((r * r + L1 * L1 - L2 * L2) / (2 * L1 * r));

    // in->sita1 = _PI_2 - sita - phi;
    // in->sita2 = _PI_2 - sita + phi;

    // /* 转换为电机正角度 */
    // *sita1 = in->sita1;
    // *sita2 = _PI - in->sita2;

    //-----------------------------------------------------------------------------------

    /* 足端偏移的虚拟腿转角 */
    float phi = atan2f(-z, x);
    if (phi > MAX_A)
    {
        phi = MAX_A;
        UART_Print("larger than the maximum angle\n");
    }
    else if (phi < MIN_A)
    {
        phi = MIN_A;
        UART_Print("less than the minimum angle\n");
    }

    /* 虚拟腿长 */
    float OD = sqrtf(z * z + x * x);
    if (OD > MAX_L)
    {
        OD = MAX_L;
        UART_Print("Longer than the longest leg\n");
    }
    else if (OD < MIN_L)
    {
        OD = MIN_L;
        UART_Print("Shorter than the shortest leg\n");
    }

    float OBD = acosf((L1 * L1 + L3 * L3 - OD * OD) / (2 * L1 * L3));
    float OC = sqrtf(L1 * L1 + L2 * L2 - 2 * L1 * L2 * cosf(OBD));
    float BOC = acosf((L1 * L1 + OC * OC - L2 * L2) / (2 * L1 * OC));
    float COD = acosf((OC * OC + OD * OD - (L3 - L2) * (L3 - L2)) / (2 * OC * OD));

    in->sita1 = phi - COD - BOC;
    in->sita2 = phi - COD + BOC;

    *sita1 = in->sita1;
    *sita2 = _PI - in->sita2;
}

/* 计算雅可比矩阵 */
void cal_jacobi(VMC_t *in)
{
    float alpha = (in->sita2 - in->sita1) / 2;
    float beta = (in->sita2 + in->sita1) / 2;

    float r = L1 * cosf(beta) + sqrtf(L2 * L2 - L1 * L1 * powf(sinf(alpha), 2));
    float r_s0 = L1 * sinf(alpha) * 0.5 + 0.5 * L1 * L1 * sinf(alpha) * cosf(alpha) / sqrtf(L2 * L2 - L1 * L1 * powf(sinf(alpha), 2));
    float r_s1 = -r_s0;

    /* x */
    // 偏x偏theta1
    in->jacobi22[0] = -sinf(beta) * r * 0.5 + cosf(beta) * r_s0;
    // 偏x偏theta2
    in->jacobi22[1] = -sinf(beta) * r * 0.5 + cosf(beta) * r_s1;
    /* z */
    // 偏z偏theta1
    in->jacobi22[2] = cosf(beta) * r * 0.5 + sinf(beta) * r_s0;
    // 偏z偏theta2
    in->jacobi22[3] = cosf(beta) * r * 0.5 + sinf(beta) * r_s1;
}

/* 计算雅可比矩阵的逆 */
uint8_t cal_invJacobi(VMC_t *in)
{
    float det;
    invet22(in->jacobi22, &det, in->invJacobi22);
    if (fabs(det) < 0.00001f)
    {
        return 0;
    }

    return 1;
}

/* 计算角速度 */
void cal_omega(VMC_t *in, float Vx, float Vz, float *alphaW, float *betaW)
{
    *alphaW = (in->invJacobi22[0] * Vx) + (in->invJacobi22[2] * Vz);
    *betaW = (in->invJacobi22[1] * Vx) + (in->invJacobi22[3] * Vz);
}

/* 计算足端速度 */
void cal_velcity(VMC_t *in, float alphaW, float betaW, float *Vx, float *Vz)
{
    *Vx = (in->jacobi22[0] * alphaW) + (in->jacobi22[2] * betaW);
    *Vz = (in->jacobi22[1] * alphaW) + (in->jacobi22[3] * betaW);
}

/* 计算力矩 */
void cal_torque(VMC_t *in, float forceX, float forceZ, float *torqueF, float *torqueB)
{
    *torqueF = (-in->jacobi22[0] * forceX) + (-in->jacobi22[1] * forceZ);
    *torqueB = (-in->jacobi22[2] * forceX) + (-in->jacobi22[3] * forceZ);
}
